#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "earth.h"
#include "rotation.h"
#include "../include/1earthMyDefine.h"
#include "../include/1mathMyDefine.h"
#include "../include/initial.h"
#include "../include/class.h"
#include "../include/GINS.h"
#include "../include/file.h"
using namespace std;
using namespace Eigen;

/*
 * check T of imu
 * */
bool check_T_imu(istream& inFile) {
    double imuDataALine[7];
    inFile.read((char *)imuDataALine, 56);
    double time1 = getImuData(imuDataALine, 1).getTime();
    inFile.read((char *)imuDataALine, 56);
    double time2 = getImuData(imuDataALine, 1).getTime();
    double T_get = time2 - time1;
    if ((T_get - T_imu > (T_imu / 10.0)) || (T_get - T_imu < -(T_imu / 10.0))){
        std::cout << "Please check your T_imu, the calculate T is not equal the setting T!" << std::endl;
        return false;
    }
    else {
        return true;
    }
}


/*
 * switch which imu error need to add in the error state vector
 * */
void swit(int switch_off1[], int& n1){// 数组传个屁的引用，因为数组本身就传的是首地址，如果真的要使用引用，也要设置为 int* &
    std::cout << "误差状态向量一共有24纬度" << endl;
    std::cout << "0 is gb_x" << std::endl;
    std::cout << "1 is gb_y" << std::endl;
    std::cout << "2 is gb_z" << std::endl;
    std::cout << "3 is ab_x" << std::endl;
    std::cout << "4 is ab_y" << std::endl;
    std::cout << "5 is ab_z" << std::endl;
    std::cout << "6 is gs_x" << std::endl;
    std::cout << "7 is gs_y" << std::endl;
    std::cout << "8 is gs_z" << std::endl;
    std::cout << "9 is as_x" << std::endl;
    std::cout << "10 is as_y" << std::endl;
    std::cout << "11 is as_z" << std::endl;
    std::cout << "12 is gn_y2x" << std::endl;
    std::cout << "13 is gn_z2x" << std::endl;
    std::cout << "14 is gn_x2y" << std::endl;
    std::cout << "15 is gn_z2y" << std::endl;
    std::cout << "16 is gn_x2z" << std::endl;
    std::cout << "17 is gn_y2z" << std::endl;
    std::cout << "18 is an_y2x" << std::endl;
    std::cout << "19 is an_z2x" << std::endl;
    std::cout << "20 is an_x2y" << std::endl;
    std::cout << "21 is an_z2y" << std::endl;
    std::cout << "22 is an_x2z" << std::endl;
    std::cout << "23 is an_y2z" << std::endl;
    std::cout << "Please input the number of the state that you don't want to expand in,";
    std::cout << " and input -1 to quit!" << std::endl;
    int input_num = 0;
    int count = 0;
    while (switch_off1[count] != -1){
        count++;
    }

    /*排序*/
    int count1 = 0;
    for (int i = 0; i < 24; i++){
        if (switch_off1[i] == -1){
            count1 = i;
            break;
        }
    }
    for (int k = 0; k < count1; k++){//冒泡排序
        for (int i = 0; i < count1; i++){
            if (switch_off1[i] < switch_off1[i + 1]){
                int middle = switch_off1[i];
                switch_off1[i] = switch_off1[i + 1];
                switch_off1[i + 1] = middle;
            }
        }
    }

    /*show*/
    n1 = 33 - count;
    for (int  i = 0; i < 24; i++){
        cout << switch_off1[i] << endl;
    }

}

/*
 * initialStateBefore
 * */
void initialStateBefore(State& stateBefore,
                        const IMUDATA& imuDataBefore){
    /*开始的状态，开始的状态也就是stateBefore*/
    State stateStart(Vector3d(init_roll,
                              init_pitch,
                              init_heading),
                     Vector3d(init_v_n,
                              init_v_e,
                              init_v_d),
                     Vector3d(init_latitude,
                              init_longitude,
                              init_height),
                     init_time);
    /*使得初始状态和imu数据对齐*/
    Vector3d attitude;
    attitude = stateStart.get_att();
    Vector3d velocity;
    velocity = stateStart.get_vel();
    Vector3d position;
    position = stateStart.get_pos();
    double time;
    time = imuDataBefore.getTime();
    stateStart.changeState(attitude,
                           velocity,
                           position,
                           time);
    /*get stateBefore*/
    stateBefore = stateStart;
}

/*
 * initial_P_Q, need to change
 * */
void initial_Matrix(Vector<double, Dynamic>& delta_kBefore,
                    Vector<double, 24>& error_gyro_accel,
                    Matrix<double, Dynamic, Dynamic>& p_kBefore,
                    Matrix<double, Dynamic, Dynamic>& q){
    /*initial delta_kBefore*/
    delta_kBefore = Vector<double, Dynamic>::Zero(n);

    /*initial error_gyro_accel*/
    error_gyro_accel = init_imu_error;

    /*初始化p_kBefore*/
    Matrix<double, 33, 33> p_kBefore_33 = Matrix<double, 33, 33>::Zero();
    for (int row = 0; row < 33; row++){
        p_kBefore_33(row, row) = std_Dev[row];
    }

    //use switch_off
     for (int i = 0; i < 24; i++){
        if (switch_off[i] >= 0 && switch_off[i] <= 23){
            //如果不是最后一行或者最后一列，那么从那一行或者那一列开始，全部往左移和上移，最后使得所有要用到的行和列全部都在前n行和前n列
            if (switch_off[i] != 23){
                for (int k = switch_off[i] + 9; k < 32; k++){
                    p_kBefore_33.row(k) = p_kBefore_33.row(k + 1);
                }
                for (int k = switch_off[i] + 9; k < 32; k++){
                    p_kBefore_33.col(k) = p_kBefore_33.col(k + 1);
                }
            }
        }
        else{
            break;
        }
    }
    p_kBefore = p_kBefore_33.block(0, 0, n, n);

    // 初始化q阵，q阵一般是不变的
    Matrix<double, 30, 30> q_30 = Matrix<double, 30, 30>::Zero();
    q_30.block(0, 0, 3, 3) = VRW * VRW * I_3;
    q_30.block(3, 3, 3, 3) = ARW * ARW * I_3;
    q_30.block(6, 6, 3, 3) = 2 * gbStd * gbStd * Tgb_reci * I_3;
    q_30.block(9, 9, 3, 3) = 2 * abStd * abStd * Tab_reci * I_3;
    q_30.block(12, 12, 3, 3) = 2 * gsStd * gsStd * Tgs_reci * I_3;
    q_30.block(15, 15, 3, 3) = 2 * asStd * asStd * Tas_reci * I_3;
    q_30.block(18, 18, 3, 3) = 2 * gnStd * gnStd * Tgn_reci * I_3;
    q_30.block(21, 21, 3, 3) = 2 * gmaStd * gmaStd * Tgma_reci * I_3;
    q_30.block(24, 24, 3, 3) = 2 * anStd * anStd * Tan_reci * I_3;
    q_30.block(27, 27, 3, 3) = 2 * amaStd * amaStd * Tama_reci * I_3;
    //use switch_off
    for (int i = 0; i < 24; i++){
        if (switch_off[i] >= 0 && switch_off[i] <= 23){
            //如果不是最后一行或者最后一列，那么从那一行或者那一列开始，全部往左移和上移，最后使得所有要用到的行和列全部都在前n行和前n列
            if (switch_off[i] != 23){
                for (int k = switch_off[i] + 6; k < 29; k++){
                    q_30.row(k) = q_30.row(k + 1);
                }
                for (int k = switch_off[i] + 6; k < 29; k++){
                    q_30.col(k) = q_30.col(k + 1);
                }
            }
        }
        else{
            break;
        }
    }
    q = q_30.block(0, 0, n - 3, n - 3);
}




/*
 * blh 2 ned
 * */
Vector3d blh2ned(Vector3d position){
    double latitude = position[0];
    double longitude = position[1];
    double h = position[2];
    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(latitude);
    double rm = rmn[0];
    double rn = rmn[1];
    double rN = (rm + h) * latitude;
    double rE = (rn + h) * cos(latitude) * longitude;
    double rd = -1.0 * h;
    return Vector3d(rN, rE, rd);
}




/*
 * getAntisymmetricMatrix
 * */
Matrix3d getAntisymmetricMatrix(Vector3d a){
    Matrix3d aM;
    aM <<           0, -1 * a[2],      a[1],
                 a[2],         0, -1 * a[0],
            -1 * a[1],      a[0],         0;
    return  aM;
}





/*
 * 预测
 * */
void  pred(Matrix<double, Dynamic, Dynamic>& p_k_kBefore,
           Vector<double, Dynamic>& delta_k_kBefore,
           Vector<double, Dynamic> delta_kBefore,
           Matrix<double, Dynamic, Dynamic> p_kBefore,
           Matrix<double, Dynamic, Dynamic> q,
           State stateBefore,
           IMUDATA imuDataBefore,
           State stateNow,
           IMUDATA imuDataNow){
    Quaterniond q_bBefore2nBefore = stateBefore.get_att_quat();
    Quaterniond q_b2n = stateNow.get_att_quat();
    double T = abs(imuDataNow.getTime() - imuDataBefore.getTime());
    Matrix<double, Dynamic, Dynamic> F = getF(stateBefore,
                                              q_bBefore2nBefore,
                                              imuDataBefore,
                                              q_b2n,
                                              imuDataNow);
    //前一时刻四元数得到前一时刻的方向余弦矩阵
    Matrix3d c_bBefore2nBefore = Rotation::quaternion2matrix(q_bBefore2nBefore);
    //当前时刻四元数得到当前时刻的方向余弦矩阵
    Matrix3d c_b2n = Rotation::quaternion2matrix(q_b2n);
    /*get fai_k_k-1*/
    Matrix<double, Dynamic, Dynamic> fai_k_kBefore;
    fai_k_kBefore = Matrix<double, Dynamic, Dynamic>::Identity(n, n) + T * F;

    /*get g_kBefore*/
    Matrix<double, 33, 30> G_kBefore_33_30 = Matrix<double, 33, 30>::Zero();
    G_kBefore_33_30.block(3, 0, 3, 3) = c_bBefore2nBefore;
    G_kBefore_33_30.block(6, 3, 3, 3) = c_bBefore2nBefore;
    G_kBefore_33_30.block(9, 6, 3, 3) = I_3;
    G_kBefore_33_30.block(12, 9, 3, 3) = I_3;
    G_kBefore_33_30.block(15, 12, 3, 3) = I_3;
    G_kBefore_33_30.block(18, 15, 3, 3) = I_3;
    G_kBefore_33_30.block(21, 18, 6, 6) = I_6;
    G_kBefore_33_30.block(27, 24, 6, 6) = I_6;

    /*get G_k*/
    Matrix<double, 33, 30> G_k_33_30 = Matrix<double, 33, 30>::Zero();
    G_k_33_30.block(3, 0, 3, 3) = c_b2n;
    G_k_33_30.block(6, 3, 3, 3) = c_b2n;
    G_k_33_30.block(9, 6, 3, 3) = I_3;
    G_k_33_30.block(12, 9, 3, 3) = I_3;
    G_k_33_30.block(15, 12, 3, 3) = I_3;
    G_k_33_30.block(18, 15, 3, 3) = I_3;
    G_k_33_30.block(21, 18, 6, 6) = I_6;
    G_k_33_30.block(27, 24, 6, 6) = I_6;
    //use switch_off
    for (int i = 0; i < 24; i++){
        if (switch_off[i] >= 0 && switch_off[i] <= 23){
            //如果不是最后一行或者最后一列，那么从那一行或者那一列开始，全部往左移和上移，最后使得所有要用到的行和列全部都在前n行和前n列
            if (switch_off[i] != 23){
                for (int k = switch_off[i] + 9; k < 32; k++){
                    G_kBefore_33_30.row(k) = G_kBefore_33_30.row(k + 1);
                    G_k_33_30.row(k) = G_k_33_30.row(k + 1);
                }
                for (int k = switch_off[i] + 6; k < 29; k++){
                    G_kBefore_33_30.col(k) = G_kBefore_33_30.col(k + 1);
                    G_k_33_30.col(k) = G_k_33_30.col( k + 1);
                }
            }
        }
        else{
            break;
        }
    }
    Matrix<double, Dynamic, Dynamic> G_kBefore(n, n - 3);
    G_kBefore = G_kBefore_33_30.block(0, 0, n, n -3);
    Matrix<double, Dynamic, Dynamic> G_k(n, n - 3);
    G_k = G_k_33_30.block(0, 0, n, n - 3);

    /*get Q_kBefore*/
    Matrix<double, Dynamic, Dynamic> Q_kBefore;
    Q_kBefore = 1.0 / 2.0 * T * (fai_k_kBefore * G_kBefore * q * (G_kBefore.transpose()) * (fai_k_kBefore.transpose()) +
                                 G_k * q * (G_k.transpose()));

    /*求出delta_k_k-1*/
    delta_k_kBefore = fai_k_kBefore * delta_kBefore;

    /*求出p_k_k-1*/
    p_k_kBefore = fai_k_kBefore * p_kBefore * (fai_k_kBefore.transpose())
                  + Q_kBefore;
}







/*
 * 量测更新
 * */
void MeasUpdate(Matrix<double, Dynamic, Dynamic>& p_k,
                Vector<double, Dynamic>& delta_k,
                Matrix<double, Dynamic, Dynamic> p_k_kBefore,
                Vector<double, Dynamic> delta_k_kBefore,
                State stateNow,
                GNSSDATA gnssData){
    Quaterniond q_b2n = stateNow.get_att_quat();
    //当前时刻四元数得到当前时刻的方向余弦矩阵
    Matrix3d c_b2n = Rotation::quaternion2matrix(q_b2n);
    /*get H_k*/
    Matrix<double, 3, 33> H_k_33;
    H_k_33 << I_3, zero, getAntisymmetricMatrix(c_b2n * l2b), zero, zero, zero, zero, zero, zero, zero, zero;
    //use switch_off
    for (int i = 0; i < 24; i++){
        if (switch_off[i] >= 0 && switch_off[i] <= 23){
            //如果不是最后一行或者最后一列，那么从那一行或者那一列开始，全部往左移和上移，最后使得所有要用到的行和列全部都在前n行和前n列
            for (int k = switch_off[i] + 9; k < 32; k++){
                H_k_33.col(k) = H_k_33.col(k + 1);
            }
        }
        else{
            break;
        }
    }
    Matrix<double, Dynamic, Dynamic> H_k(3, n);
    H_k = H_k_33.block(0, 0, 3, n);

    /*get DR,用gnss结果解算rm，rn更加准确*/
    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(stateNow.getLatitude());
    double rm = rmn[0];
    double rn = rmn[1];
    Matrix3d DR;
    DR <<   rm + stateNow.getHeight(), 0, 0,
            0, (rn + stateNow.getHeight()) * cos(stateNow.getLatitude()), 0,
            0, 0, -1;

    /*get delta_Zr*/
    Vector3d rG_pre = stateNow.get_pos() + (DR.inverse()) * c_b2n * l2b;
    Vector3d rG_gnss = gnssData.getposition();
    Vector3d delta_zr = DR * (rG_pre - rG_gnss);

    /*get R_k,gnss观测噪声方差阵*/
    Matrix3d R_k;
    R_k << pow(gnssData.getLatitudeStd(), 2),   0, 0,
            0, pow(gnssData.getLongitudeStd(), 2), 0,
            0, 0, pow(gnssData.getHeightStd(), 2);

    /*get K_K*/
    Matrix <double, Dynamic, Dynamic> k_k;
    k_k = p_k_kBefore * (H_k.transpose()) * ((H_k * p_k_kBefore * H_k.transpose() + R_k).inverse());

    /*get delta_k*/
    delta_k = k_k * delta_zr;

    /*求出p阵的最优估计值*/
    p_k = (Matrix<double, Dynamic, Dynamic>::Identity(n, n) - k_k * H_k) * p_k_kBefore
          * ((Matrix<double, Dynamic, Dynamic>::Identity(n, n) - k_k * H_k).transpose()) + k_k * R_k * (k_k.transpose());
}






/*
 * 校正输出数据
 * */
void correctOut(State& stateNow,
                Vector<double, Dynamic>& delta_k){

    /*对imu输出数据进行校正*/
    Vector3d delta_r = Vector3d(delta_k[0], delta_k[1], delta_k[2]);
    Vector3d delta_v = Vector3d(delta_k[3], delta_k[4], delta_k[5]);
    Vector3d delta_seta = Vector3d(delta_k[6], delta_k[7], delta_k[8]);

    /*校正位置*/
    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(stateNow.getLatitude());
    double rm = rmn[0];
    double rn = rmn[1];
    double latitude = stateNow.getLatitude();
    double longitude = stateNow.getLongitude();
    double h = stateNow.getHeight();

    double rN = (rm + h) * latitude;
    double rE = (rn + h) * cos(latitude) * longitude;
    double rd = -1.0 * h;

    double rN_cor = rN - delta_r[0];
    double rE_cor = rE - delta_r[1];
    double rd_cor = rd - delta_r[2];

    double h_cor = -1.0 * rd_cor;
    double latitude_cor = rN_cor / (rm + h);///////////存疑
    double longitude_cor = rE_cor / (rn + h) / cos(latitude);//////////存疑
    Vector3d position =Vector3d(latitude_cor, longitude_cor, h_cor);

    /*校正速度*/
    Vector3d velocity = stateNow.get_vel() - delta_v;

    /*校正四元数*/
    Matrix3d c_b2n = Rotation::quaternion2matrix(stateNow.get_att_quat());
    c_b2n = ((I_3 - getAntisymmetricMatrix(delta_seta)).inverse()) * c_b2n;
    Quaterniond quaterniond = Rotation::matrix2quaternion(c_b2n);

    /*校正状态*/
    stateNow.changeState(quaterniond,
                         velocity,
                         position,
                         stateNow.getTime());

    /*校正之后对delta_k中的delta_r,delta_v,delta_seta清零*/
    for (int i = 0; i < 9; i++){
        delta_k[i] = 0;
    }
}






/*
 * get the imu error of gyro and accel
 * */
void getErrorGA(Vector<double, Dynamic>& delta_k,
                Vector<double, 24>& error_gyro_accel) {
    /*对switch_off中进行倒序，从小到大*/
    int switch_off1[24];
    int count = 0;
    for (int i = 0; i < 24; i++) {
        switch_off1[i] = -1;
    }
    for (int i = 0; i < 24; i++) {
        if (switch_off[i] == -1) {
            count = i;
            break;
        }
    }
    if (count != 0) {
        for (int i = 0; i < count; i++) {
            switch_off1[i] = switch_off[count - i - 1];
        }
    }
    /*得到完整的误差状态矩阵*/
    //先将delta_k中的所有元素都复制到delta中
    Vector<double, 24> delta = Vector<double, 24>::Zero();
    for (int i = 0; i < n - 9; i++) {
        delta(i) = delta_k(i + 9);
    }
    //将误差状态矩阵全部置零
    delta_k = Vector<double, Dynamic>::Zero(n);
    //如果k出现在switch_off1中的话，代表这未被曾广进来的，因此需要从最后一个元素开始到第k个依次向后移位，然后在将delta_k[k]变成0
    for (int i = 0; i < 24; i++) {
        if (switch_off1[i] != -1) {
            for (int m = 23; m > switch_off1[i]; m--) {
                delta(m) = delta(m - 1);
            }
            delta(switch_off1[i]) = 0;
        }
        else {
            break;
        }
    }
    for (int i = 0; i < 6; i++){
        error_gyro_accel[i] += delta[i];// + delta[i] * error_gyro_accel[i + 6];
    }
    for (int i = 6; i < 12; i++){
        error_gyro_accel[i] += delta[i];// + delta[i] * error_gyro_accel[i];
    }
    for (int i = 12; i < 24; i++){
        error_gyro_accel[i] += delta[i];
    }
}

/*
 * 矫正 IMU， need to change
 * */
void correctGyroAccel(Vector<double, 24> error_gyro_accel,
                      IMUDATA& imuDataNow,
                      IMUDATA imuDataBefore){
    //对当前时刻imu陀螺仪输出进行校正，好像不会对前一时刻进行校正
    double T = abs(imuDataNow.getTime() - imuDataBefore.getTime());
    // bias
    Vector3d gb = Vector3d(error_gyro_accel[0], error_gyro_accel[1], error_gyro_accel(2));
    Vector3d ab = Vector3d(error_gyro_accel[3], error_gyro_accel[4], error_gyro_accel(5));

    // scale
    Matrix3d gs;
    gs << error_gyro_accel[6],                    0,                   0,
            0,                  error_gyro_accel[7],                   0,
            0,                                    0, error_gyro_accel[8];
    Matrix3d as;
    as << error_gyro_accel[9],                  0,                    0,
            0,               error_gyro_accel[10],                    0,
            0,                                  0, error_gyro_accel[11];

    // 现在的交轴耦合误差就包含了两个方面，非正交误差和安装角误差；但是二者的处事不确定度和误差参数是一致的，所以这里可以不改
    // if gyro use up triangle matrix，12 is ny2x； 13 is nz2x; 14 is nz2y
    // if gyro use down triangle matrix, 12 is nx2y; 13 is nx2z; 14 is ny2z
    // 15 is fai_x, 16 is fai_y, 17 is fai_z
    // 现在的交轴耦合误差就包含了两个方面，非正交误差和安装角误差；但是二者的处事不确定度和误差参数是一致的，所以这里可以不改
    // if gyro use up triangle matrix，18 is ny2x； 19 is nz2x; 20 is nz2y
    // if gyro use down triangle matrix, 18 is nx2y; 19 is nx2z; 20 is ny2z
    // 21 is fai_x, 22 is fai_y, 23 is fai_z
    Matrix3d gyro_non_matrix;
    Matrix3d gyro_project_matrix;
    Matrix3d accel_non_matrix;
    Matrix3d accel_project_matrix;
    // 陀螺仪交轴耦合
    if (gyro_non_mode == gyro_up_triangle) {
        gyro_non_matrix << 0, error_gyro_accel[12], error_gyro_accel[13],
                           0, 0, error_gyro_accel[14],
                           0, 0, 0;
    }
    else if ( gyro_non_mode == gyro_down_triangle) {
        gyro_non_matrix << 0, 0, 0,
                           error_gyro_accel[12], 0, 0,
                           error_gyro_accel[13], error_gyro_accel[14], 0;
    }

    // 加表交轴耦合
    if (accel_non_mode == accel_up_triangle) {
        accel_non_matrix << 0, error_gyro_accel[18], error_gyro_accel[19],
                            0, 0, error_gyro_accel[20],
                            0, 0, 0;
    }
    else if (accel_non_mode == accel_down_triangle) {
        accel_non_matrix << 0, 0, 0,
                            error_gyro_accel[18], 0, 0,
                            error_gyro_accel[19], error_gyro_accel[20], 0;
    }

    // 投影模型
    if (project_mode == project2gyro) {
        gyro_project_matrix = Eigen::MatrixXd::Zero(3, 3);
        accel_project_matrix << 0, -error_gyro_accel[23], error_gyro_accel[22],
                                error_gyro_accel[23], 0, -error_gyro_accel[21],
                                -error_gyro_accel[22], error_gyro_accel[21], 0;
    }
    else if (project_mode == project2accel) {
        gyro_project_matrix << 0, -error_gyro_accel[17], error_gyro_accel[16],
                               error_gyro_accel[17], 0, -error_gyro_accel[15],
                               -error_gyro_accel[16], error_gyro_accel[15], 0;
        accel_project_matrix = Eigen::MatrixXd::Zero(3, 3);
    }
    else if (project_mode == noproject) {
        gyro_project_matrix = Eigen::MatrixXd::Zero(3, 3);
        accel_project_matrix = Eigen::MatrixXd::Zero(3, 3);
    }

    // gn
    // an
    Matrix3d gn;
    Matrix3d an;
    gn = gyro_non_matrix + gyro_project_matrix + I_3;
    an = accel_non_matrix + accel_project_matrix + I_3;

    // change
    imuDataNow.changeGyroData(((gs + gn).inverse()) * (imuDataNow.getGyroData() - gb * T));
    imuDataNow.changeAccelData(((as + an).inverse()) * (imuDataNow.getAccelData() - ab * T));
}

/*
 * 内插，这个函数似乎始终没有用到过
 * */
State interPolate(State stateBefore,
                  State stateNext,
                  GNSSDATA gnssData,
                  double t1,
                  double t2,
                  double t){
    State stateNow;
    Vector3d positionNow;
    positionNow = stateBefore.get_pos()
                  + (stateNext.get_pos() - stateBefore.get_pos()) * t1 / t;
    Vector3d velocityNow;
    velocityNow = stateBefore.get_vel()
                  + (stateNext.get_vel() - stateBefore.get_vel()) * t1 / t;
    Vector3d attitudeNow;////??????姿态如何进行内插
    attitudeNow = stateBefore.get_att()
                  + (stateNext.get_att() - stateBefore.get_att()) * t1 / t;
    double timeNow;
    timeNow = gnssData.getTime();
    /*得到当前时刻的state*/
    stateNow = State(attitudeNow, velocityNow, positionNow, timeNow);
    return stateNow;
}


extern const Matrix3d zero;

/*
 * get sec function
 * */
double sec(double x){
    return 1.0 / cos(x);
}

/*
 * get 对角矩阵
 * */
extern Matrix3d getAntisymmetricMatrix(Vector3d a);
Matrix3d diag(Vector3d a){
    Matrix3d diagA;
    diagA << a[0], 0, 0,
            0, a[1], 0,
            0, 0, a[2];
    return diagA;
}

/*
 * get F阵, need to change
 * */
Matrix<double, Dynamic, Dynamic> getF(State stateBefore,
                                      Quaterniond q_bBefore2nBefore,
                                      IMUDATA imuDataBefore,
                                      Quaterniond q_b2n,
                                      IMUDATA imuDataNow){
    Vector3d v = stateBefore.get_vel();
    double vN = stateBefore.getVNorth();
    double vE = stateBefore.getVEast();
    double vD = stateBefore.getVDown();

    Vector3d position = stateBefore.get_pos();
    double latitude = stateBefore.getLatitude();
    double fai = latitude;
    double lambda = stateBefore.getLongitude();//我将当前时刻的经度和高度改成了前一时刻的经度和高度
    double h = stateBefore.getHeight();
    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(latitude);
    double rm = rmn[0];
    double rn = rmn[1];

    double gl = Earth::gravity(position);
    double gp = gl;

    //当前时刻四元数得到当前时刻的方向余弦矩阵
    Matrix3d c_b2n = Rotation::quaternion2matrix(q_bBefore2nBefore);

    Vector3d fb = imuDataBefore.getAccelData() / (imuDataNow.getTime() - imuDataBefore.getTime());//其实这里应该是前一时时间减去前两个时刻时间
    Vector3d w_ib2b = imuDataBefore.getGyroData() / (imuDataNow.getTime() - imuDataBefore.getTime());

    Vector3d w_ie2n = Earth::iewn(latitude);
    Vector3d w_en2n = Earth::enwn(rmn, position, v);
    Vector3d w_in2n = w_ie2n + w_en2n;



    Matrix3d Frr;
    Frr <<      -1.0 * vD / (rm + h),                                       0, vN / (rm + h),
            vE * tan(fai) / (rn + h), - 1.0 * (vD + vN * tan(fai)) / (rn + h), vE / (rn + h),
            0, 0, 0;

    Matrix3d Fvr;
    Fvr(0, 0) = -2.0 * vE * we * cos(fai) / (rm + h)
                - (vE * vE * sec(fai) * sec(fai)) / (rm + h) / (rn + h);
    Fvr(0, 1) = 0;
    Fvr(0, 2) = vN * vD / ( rm + h) / (rm + h)
                - vE * vE * tan(fai) / (rn + h) / (rn + h);
    Fvr(1, 0) = 2.0 * we * (vN * cos(fai) - vD * sin(fai)) / (rm + h)
                + vN * vE * sec(fai) * sec(fai) / (rm + h) / (rn + h);
    Fvr(1, 1) = 0;
    Fvr(1, 2) = (vE * vD + vN * vE * tan(fai)) / (rn + h) / (rn + h);
    Fvr(2, 0) = 2.0 * we * vE * sin(fai) / (rm + h);
    Fvr(2, 1) = 0;
    Fvr(2, 2) = -1.0 * vE * vE  / (rn + h) / (rn + h)
                - vN * vN / (rm + h) / (rm + h)
                + 2 * gp / (sqrt(rm * rn) + h);

    Matrix3d Fvv;
    Fvv(0, 0) = vD / (rm +h);
    Fvv(0, 1) = -2.0 * (we * sin(fai) + vE * tan(fai) / (rn + h));
    Fvv(0, 2) = vN / (rm + h);
    Fvv(1, 0) = 2.0 * we * sin(fai) + vE * tan(fai) / (rn + h);
    Fvv(1, 1) = (vD + vN * tan(fai)) / (rn + h);
    Fvv(1, 2) = 2.0 * we * cos(fai) + vE / (rn + h);
    Fvv(2, 0) = -2.0 * vN / (rm + h);
    Fvv(2, 1) = -2.0 * (we * cos(fai) + vE / (rn + h));
    Fvv(2, 2) = 0;

    Matrix3d F_fai_r;
    F_fai_r(0, 0) = -1.0 * we * sin(fai) / (rm + h);
    F_fai_r(0, 1) = 0;
    F_fai_r(0, 2) = vE / (rn +h) / (rn +h);
    F_fai_r(1, 0) = 0;
    F_fai_r(1, 1) = 0;
    F_fai_r(1, 2) = -1.0 * vN /(rm + h) / (rm + h);
    F_fai_r(2, 0) = -1.0 * we * cos(fai) / (rm + h)
                    - vE * sec(fai) * sec(fai) / (rm + h) / (rn +h);
    F_fai_r(2, 1) = 0;
    F_fai_r(2, 2) = -1.0 * vE * tan(fai) / (rn + h) / (rn + h);

    Matrix3d F_fai_v;
    F_fai_v(0, 0) = 0;
    F_fai_v(0, 1) = 1.0 / (rn + h);
    F_fai_v(0, 2) = 0;
    F_fai_v(1, 0) = -1.0 / (rm + h);
    F_fai_v(1, 1) = 0;
    F_fai_v(1, 2) = 0;
    F_fai_v(2, 0) = 0;
    F_fai_v(2, 1) = -1.0 * tan(fai) / (rn + h);
    F_fai_v(2, 2) = 0;

    double w_ib2b_x = w_ib2b[0];
    double w_ib2b_y = w_ib2b[1];
    double w_ib2b_z = w_ib2b[2];
    double fb_x = fb[0];
    double fb_y = fb[1];
    double fb_z = fb[2];

    Matrix<double, 3, 6> gn_matrix = Matrix<double, 3, 6>::Zero();
    Matrix<double, 3, 6> an_matrix = Matrix<double, 3, 6>::Zero();

    // 陀螺仪交轴耦合
    if (gyro_non_mode == gyro_up_triangle) {
        gn_matrix(0, 0) = w_ib2b_y;
        gn_matrix(0, 1) = w_ib2b_z;
        gn_matrix(1, 2) = w_ib2b_z;
    }
    else if ( gyro_non_mode == gyro_down_triangle) {
        gn_matrix(1, 0) = w_ib2b_x;
        gn_matrix(2, 1) = w_ib2b_x;
        gn_matrix(2, 2) = w_ib2b_y;
    }

    // 加表交轴耦合
    if (accel_non_mode == accel_up_triangle) {
        an_matrix(0, 0) = fb_y;
        an_matrix(0, 1) = fb_z;
        an_matrix(1, 2) = fb_z;
    }
    else if (accel_non_mode == accel_down_triangle) {
        an_matrix(1, 0) = fb_x;
        an_matrix(2, 1) = fb_x;
        an_matrix(2, 2) = fb_y;
    }

    // project_mode
    if (project_mode == project2gyro) {
        an_matrix(0, 4) = fb_z;
        an_matrix(0, 5) = -fb_y;
        an_matrix(1, 3) = -fb_z;
        an_matrix(1, 5) = fb_x;
        an_matrix(2, 3) = fb_y;
        an_matrix(2, 4) = -fb_x;
    }
    else if (project_mode == project2accel) {
        gn_matrix(0, 4) = w_ib2b_z;
        gn_matrix(0, 5) = -w_ib2b_y;
        gn_matrix(1, 3) = -w_ib2b_z;
        gn_matrix(1, 5) = w_ib2b_x;
        gn_matrix(2, 3) = w_ib2b_y;
        gn_matrix(2, 4) = -w_ib2b_x;
    }

    Matrix<double, 33, 33> F_33 = Matrix<double, 33, 33>::Zero();
    F_33 <<     Frr,     I_3,                                  zero,         zero,  zero,                        zero,             zero, zero, zero, zero, zero,
                Fvr,     Fvv,    getAntisymmetricMatrix(c_b2n * fb),         zero, c_b2n,                        zero, c_b2n * diag(fb), zero, zero, zero, zero,
            F_fai_r, F_fai_v, -1.0 * getAntisymmetricMatrix(w_in2n), -1.0 * c_b2n,  zero, -1.0 * c_b2n * diag(w_ib2b),             zero, zero, zero, zero, zero,
            zero, zero, zero, -Tgb_reci * I_3, zero, zero, zero, zero, zero, zero, zero,
            zero, zero, zero, zero, -Tab_reci * I_3, zero, zero, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, -Tgs_reci * I_3, zero, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, zero, -Tas_reci * I_3, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, zero,            zero, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, zero,            zero, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, zero,            zero, zero, zero, zero, zero,
            zero, zero, zero, zero, zero, zero,            zero, zero, zero, zero, zero;
    F_33.block(3, 27, 3, 6) =        c_b2n * an_matrix;
    F_33.block(6, 21, 3, 6) = -1.0 * c_b2n * gn_matrix;
    F_33.block(21, 21, 3, 3) = -Tgn_reci * I_3;
    F_33.block(24, 24, 3, 3) = -Tgma_reci * I_3;
    F_33.block(27, 27, 3, 3) = -Tan_reci * I_3;
    F_33.block(30, 30, 3, 3) = -Tama_reci * I_3;

    //use switch_off
    for (int i = 0; i < 24; i++){
        if (switch_off[i] >= 0 && switch_off[i] <= 23){
            //如果不是最后一行或者最后一列，那么从那一行或者那一列开始，全部往左移和上移，最后使得所有要用到的行和列全部都在前n行和前n列
            if (switch_off[i] != 23){
                for (int k = switch_off[i] + 9; k < 32; k++){
                    F_33.row(k) = F_33.row(k + 1);
                }
                for (int k = switch_off[i] + 9; k < 32; k++){
                    F_33.col(k) = F_33.col(k + 1);
                }
            }
        }
        else{
            break;
        }
    }
    Matrix<double, Dynamic, Dynamic> F(n, n);
    F = F_33.block(0, 0, n, n);
    return F;
}